import pygame
import binascii
import os
import time
import socket
import sys
import threading
import struct
import asyncio
import cv2
import numpy as np
from car_controller import CarController

pygame_ready = False
def getKeyState():
	pygame.init()
	screen = pygame.display.set_mode((640, 480))
	pygame.display.set_caption('Car controlling')
	global cur_key, img, pygame_ready
	pygame_ready = True
	while not cur_key[pygame.K_ESCAPE]:
		cur_key = pygame.key.get_pressed()
		pygame.event.pump()
		# print('getKeyState()::', img is None)
		if img is not None:
			# print('getKeyState()::img blit here')
			pygame.surfarray.blit_array(screen, img)
			pygame.display.update()
		pass
	pygame.quit()
	pass

img = None
def getImage():
	global img, pygame_ready
	while not pygame_ready: pass
	while not cur_key[pygame.K_ESCAPE]:
		img = car_controller.getImage()
		# print('getImage()::', img is None)
	pass

def sendCommand():
	global pygame_ready
	while not pygame_ready: pass
	while not cur_key[pygame.K_ESCAPE]:
		motor_u_d, motor_l_r = 0, 0
		if cur_key[pygame.K_UP]: motor_u_d += 1
		if cur_key[pygame.K_DOWN]: motor_u_d -= 1
		if cur_key[pygame.K_LEFT]: motor_l_r += 1
		if cur_key[pygame.K_RIGHT]: motor_l_r -= 1
		
		if motor_u_d > 0 and motor_l_r > 0:
			car_controller.motorLeftForward()
		if motor_u_d > 0 and motor_l_r == 0:
			car_controller.motorForward()
		if motor_u_d > 0 and motor_l_r < 0:
			car_controller.motorRightForward()
		if motor_u_d == 0 and motor_l_r > 0:
			car_controller.motorLeft()
		if motor_u_d == 0 and motor_l_r == 0:
			car_controller.motorStop()
		if motor_u_d == 0 and motor_l_r < 0:
			car_controller.motorRight()
		if motor_u_d < 0 and motor_l_r > 0:
			car_controller.motorLeftBackward()
		if motor_u_d < 0 and motor_l_r == 0:
			car_controller.motorBackward()
		if motor_u_d < 0 and motor_l_r < 0:
			car_controller.motorRightBackward()
	pass

pygame.init()
cur_key = pygame.key.get_pressed()
pygame.quit()

car_controller = CarController(IP=input('ip?'), image_on=False)

print('here at main before threading')
threading.Thread(target=getKeyState, name='getKeyState').start()
threading.Thread(target=sendCommand, name='sendCommand').start()
threading.Thread(target=car_controller.receiveMessage, name='receiveMessage').start()
if car_controller.image_on: threading.Thread(target=getImage, name='getImage').start()
# threading.Thread(target=receiveImage, name='receiveImage').start()
print(len(threading.enumerate()))

while len(threading.enumerate()) > 1:
	pass